#include "hins/laser_data_receiver.h"
#include "hins/protoc.h"
#include "utils.h"
#include <iostream>
#include <fstream>

// #define DEBUG
namespace hins {

LaserDataReceiver::LaserDataReceiver(const ConnectionAddress& conn_info)
  : conn_info_(conn_info)
  , is_connected_(false)
  , inbuf_(4096)
  , instream_(&inbuf_)
  , tcp_socket_ptr_(0)
  , ring_buffer_(65536)
  , scan_data_()
  , last_data_time_(hins::Now())
{
  std::cout << "Connecting to TCP data channel at hostname: " << conn_info.GetAddress()
          << " tcp_port:" << conn_info.GetPort() << std::endl;

  if(Connect())
    std::cout << "Lidar connect success." << std::endl;
  else
    std::cout << "Lidar connect failed." << std::endl;
}

LaserDataReceiver::~LaserDataReceiver()
{
  Disconnect();

  if(tcp_socket_ptr_)
    delete tcp_socket_ptr_;
}

bool LaserDataReceiver::IsConnected()
{
  return is_connected_;
}

void LaserDataReceiver::Disconnect()
{
  is_connected_ = false;
  try
  {
    if( tcp_socket_ptr_ != nullptr )
      tcp_socket_ptr_->close();

    io_service_.stop();
    if( boost::this_thread::get_id() != io_service_thread_.get_id() )
      io_service_thread_.join();

  }
  catch (std::exception& e)
  {
    std::cout << "Exception:" << e.what() << std::endl;
  }
  std::cout << "Disconnect." << std::endl;
}

bool LaserDataReceiver::CheckConnection()
{
  if( !IsConnected() )
    return false;
  if( (hins::Now() - last_data_time_) > 1000 ) // 1s 断线
  {
    Disconnect();
    std::cout << "超时断线，超时时间:" << (hins::Now() - last_data_time_)/1000.0 << std::endl;
    return false;
  }
  return true;
}

ScanData LaserDataReceiver::GetFullScan()
{
  std::unique_lock<std::mutex> lock(scan_mutex_);
  while( CheckConnection() && scan_data_.size() < 2 )
    data_notifier_.wait_for(lock, std::chrono::seconds(1));


  ScanData data;
  if( scan_data_.size() >= 2 && IsConnected())
  {
    data = ScanData(std::move(scan_data_.front()));
    scan_data_.pop_front();
  }
  else
    std::cout << "null data" << std::endl;
  return data;
}

void LaserDataReceiver::HandleSocketRead(const boost::system::error_code &error)
{
  if(!error)
  {
    // 1. 将所有数据推送到缓冲区
    instream_.clear();
    while(!instream_.eof())
    {
      char buf[4096];
      instream_.read(buf, 4096);
      int bytes_read = instream_.gcount();
      WriteBufferBack(buf, bytes_read);       // 将 buf 的数据写到 ring_buffer_ 里
    }

    // 2. 继续读取数据，直到数据被读取完
    while(HandleNextPacket()) {}

    // 3. 继续异步读取数据
    boost::asio::async_read(*tcp_socket_ptr_, inbuf_, boost::bind(&LaserDataReceiver::HandleSocketRead, this, boost::asio::placeholders::error));
  }
  else
  {
    if( error.value() != 995 )
      std::cout << "ERROR: data connection error:"
                << error.message()
                << " "
                << error.value()
                << std::endl;
    Disconnect();
  }
}

int LaserDataReceiver::SyncWrite()
{
  boost::system::error_code ec;

  int ret = tcp_socket_ptr_->write_some(boost::asio::buffer(kStartCapture), ec);
  if(ec)
  {
    std::cout << "write failed:%" << boost::system::system_error(ec).what() << std::endl;
    ret = -1;
  }
  #ifdef DEBUG
  std::cout << "SyncWrite kStartCapture done" << std::endl;
  #endif
  return ret;
}


int LaserDataReceiver::SyncWrite(const std::array<unsigned char, 12> command)
{
  boost::system::error_code ec;

  int ret = tcp_socket_ptr_->write_some(boost::asio::buffer(command), ec);
  if(ec)
  {
    std::cout << "write failed:" << boost::system::system_error(ec).what() << std::endl;
    ret = -1;
  }
  #ifdef DEBUG
  std::cout << "SyncWrite command done" << std::endl;
  #endif
  return ret;
}

// CRC循环校验码-12位命令
// 修改command的最后两位
// 返回循环校验码的10进制数字
unsigned int CRC_Verify_len12(std::array<unsigned char, 12> *command)
{
  unsigned int i, j;
  unsigned int wCrc = 0xffff;
  unsigned int wPolynom = 0xA001;
  /*-------------------------------------*/
  for (i = 0; i < 12 - 2; i++) // 后两位为校验位
  {
    wCrc ^= command->at(i);
    for (j = 0; j < 8; j++)
    {
      if (wCrc & 0x0001)
      {
        wCrc = (wCrc >> 1) ^ wPolynom;
      }
      else
      {
        wCrc = wCrc >> 1;
      }
    }
  }
  unsigned int tmp1 = (wCrc & 0xff00) >> 8;           // 高位
  unsigned int tmp2 = (wCrc & 0x00ff);                // 低位
  command->at(10) = tmp2;
  command->at(11) = tmp1;
  return wCrc;
}

bool LaserDataReceiver::Connect()
{
  try
  {
    // 创建hostname/ip
    boost::asio::ip::tcp::resolver resolver(io_service_);
    boost::asio::ip::tcp::resolver::query query(conn_info_.GetAddress(), std::to_string(conn_info_.GetPort()));
    boost::asio::ip::tcp::resolver::iterator endpoint_iterator = resolver.resolve(query);
    boost::asio::ip::tcp::resolver::iterator end;

    if(nullptr == tcp_socket_ptr_)
    {
      tcp_socket_ptr_ = new boost::asio::ip::tcp::socket(io_service_);
    }
    else
    {
      delete tcp_socket_ptr_;
      tcp_socket_ptr_ = new boost::asio::ip::tcp::socket(io_service_);
    }

    boost::system::error_code error = boost::asio::error::host_not_found;

    // 迭代端点并建立连接
    while (error && endpoint_iterator != end)
    {
      // tcp_socket_ptr_->close();
      tcp_socket_ptr_->connect(*endpoint_iterator++, error);
    }
    if (error)
    {
      std::cout << "Connect failed." << error.message().c_str() << std::endl;
      return false;
    }

    // 开始异步读取数据
    boost::asio::async_read(*tcp_socket_ptr_, inbuf_, boost::bind(&LaserDataReceiver::HandleSocketRead, this, boost::asio::placeholders::error));
    io_service_thread_ = boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
    is_connected_ = true;
  }
  catch (std::exception& e)
  {
    std::cout << "Exception:" << e.what() << std::endl;
    is_connected_ = false;
    return false;
  }
  return true;
}

// 根据协议寻找数据帧头
int32_t LaserDataReceiver::FindPacketStart()
{
  if(ring_buffer_.size() < kXSPackageHeadSize)
    return -1;

  // 兴颂LS雷达数据的帧头
  for(size_t i = 0; i < ring_buffer_.size() - 5; i++)
  {
    // 距离数据
    if(0x43 == ((unsigned char)ring_buffer_[i])   &&
       0x6c == ((unsigned char)ring_buffer_[i+1]) &&
       0x6f == ((unsigned char)ring_buffer_[i+2]) &&
       0x75 == ((unsigned char)ring_buffer_[i+3]) &&
       0x64 == ((unsigned char)ring_buffer_[i+4]))
    {
      i = i << 2;
      i |= 0x01;     // bit0置1 
      return i;
    }
    else
      // 区域数据
      if(0x57 == ((unsigned char)ring_buffer_[i])   &&
         0x53 == ((unsigned char)ring_buffer_[i+1]) &&
         0x69 == ((unsigned char)ring_buffer_[i+2]) &&
         0x6d == ((unsigned char)ring_buffer_[i+3]) &&
         0x75 == ((unsigned char)ring_buffer_[i+4]))
      {
        i = i << 2;
        i |= 0x02;     // bit1置1
        return i;
      }
  }
  return -2;
}

// 将src数据写到 ring_buffer_ 里
void LaserDataReceiver::WriteBufferBack(char *src, std::size_t num_bytes)
{
  if(ring_buffer_.size() + num_bytes > ring_buffer_.capacity())
    throw std::exception();

  ring_buffer_.resize(ring_buffer_.size() + num_bytes);           // 修改 ring_buffer_ 的大小
  char* pone = ring_buffer_.array_one().first;                    // ring_buffer_ 的 array_one 的头指针
  std::size_t pone_size = ring_buffer_.array_one().second;        // ring_buffer_ 的 array_one 的大小
  char* ptwo = ring_buffer_.array_two().first;                    // ring_buffer_ 的 array_two 的头指针
  std::size_t ptwo_size = ring_buffer_.array_two().second;        // ring_buffer_ 的 array_two 的大小

  // 将src数据写到 ring_buffer_ 里
  if(ptwo_size >= num_bytes)
  {
    std::memcpy(ptwo + ptwo_size - num_bytes, src, num_bytes);
  }
  else
  {
    std::memcpy(pone + pone_size + ptwo_size - num_bytes, src, num_bytes - ptwo_size);
    std::memcpy(ptwo, src + num_bytes - ptwo_size, ptwo_size);
  }
}

bool LaserDataReceiver::HandleNextPacket()
{
  if(scan_data_.empty())
  {
    scan_data_.emplace_back();
  }

  if(RetrivePacket())
  {
    return true;
  }
  else
  {
    return false;
  }
}


bool LaserDataReceiver::RetrivePacket()
{
  bool ret = false;
  int16_t head_index = FindPacketStart();                     // 寻找帧头
  if(head_index < 0)
    return ret;
  // std::cout << "找到帧头了" << std::endl;
  if(head_index & 0x01==1)      // 距离数据
  {
    head_index = head_index >> 2;
    // 寻找帧头并处理数据
    if(ring_buffer_.size() - head_index >= kXSPackageHeadSize)
    {
      ring_buffer_.erase_begin(head_index);                     // 删除 帧头前的数据
      head_index = 0;

      // 1. 解析数据帧帧头
      char head_buf[kXSPackageHeadSize];
      ReadBufferFront(head_buf, kXSPackageHeadSize);

      XSPackageHeader header;
      header.start_angle = (unsigned char)head_buf[6] << 8;     // 起始角度
      header.start_angle |= (unsigned char)head_buf[7];
      header.end_angle = (unsigned char)head_buf[8] << 8;       // 终止角度
      header.end_angle |= (unsigned char)head_buf[9];
      header.data_size = (unsigned char)head_buf[10] << 8;      // 测量点总数
      header.data_size |= (unsigned char)head_buf[11];

      std::unique_lock<std::mutex> lock(scan_mutex_);

      // 2. 解析帧内容
      ScanData& scan_data = scan_data_.back();
      uint16_t body_size = kXSPackageHeadSize + header.data_size * 4;

      if((ring_buffer_.size() - head_index) >= body_size)
      {
        #ifdef DEBUG
        std::cout << "begin:" << header.start_angle 
                  << "  end:" << header.end_angle 
                  << "  data_size:" << header.data_size
                  << std::endl;
        #endif

        char* body_buf = new char[body_size];
        ReadBufferFront(body_buf, body_size);                   // 将当前帧的数据复制到 body_buf 中

        ring_buffer_.erase_begin(head_index + body_size);       // 删除 ring_buffer_ 的数据

        for(int i = 0; i < header.data_size; i++)
        {
          unsigned short int distance;
          unsigned short int intensity;
          distance = (unsigned char)body_buf[i*4 + 13] << 8;
          distance |= (unsigned char)body_buf[i*4 + 12];
          intensity = (unsigned char)body_buf[i*4 + 15] << 8;
          intensity |= (unsigned char)body_buf[i*4 + 14];

          if(distance > kMaxDistance)
          {
            distance = kMaxDistance;
          }
          if(intensity > kMaxIntensity)
          {
            intensity = kMaxIntensity;
          }

          scan_data.distance_data.push_back(distance);
          scan_data.amplitude_data.push_back(intensity);
        }

        delete [] body_buf;

        if(header.end_angle == 315)
        {
          scan_data_.emplace_back();
          if(scan_data_.size() > 5)
          {
            scan_data_.pop_front();
            std::cout << "buffer data too many, drops it" << std::endl;
          }
          data_notifier_.notify_one();
          last_data_time_ = hins::Now();
          ret = true;
        }

        if(FindPacketStart() >= 0)
        {
          ret = true;
        }
      }
    }
    else
      if(head_index & 0x02==1)      // 区域数据
      {
        head_index = head_index >> 2;
        if(ring_buffer_.size() - head_index >= kXSAreaDataSize)
        {
          AreaData area_data;
          char area_buf[kXSPackageHeadSize];
          ring_buffer_.erase_begin(head_index);                     // 删除 帧头前的数据
          ReadBufferFront(area_buf, kXSPackageHeadSize);
          area_data.channel = (unsigned char)area_buf[5];           // 传感器通道值
          area_data.input_state = (unsigned char)area_buf[6];
          area_data.output_state = (unsigned char)area_buf[7];
          area_data.led_state = (unsigned char)area_buf[8];
          area_data.error_state = (unsigned char)area_buf[9];
          area_data.check = (unsigned char)area_buf[9];
        }
      }
  }
  return ret;
}

// 将ring_buffer_前num_bytes的数据拷贝到dst
void LaserDataReceiver::ReadBufferFront(char *dst, const uint16_t &num_bytes)
{
  if( ring_buffer_.size() < num_bytes )
    throw std::exception();

  char* pone = ring_buffer_.array_one().first;                  // 指向环形缓冲区ring_buffer_的array_one的开始地址
  std::size_t pone_size = ring_buffer_.array_one().second;      // ring_buffer_的array_one的大小
  char* ptwo = ring_buffer_.array_two().first;                  // 指向环形缓冲区ring_buffer_的array_two的开始地址

  if( pone_size >= num_bytes )                                  // ring_buffer_的array_one的大小大于num_bytes
  {
    std::memcpy(dst, pone, num_bytes);                          // 复制环形缓冲区的数据到dst中
  }
  else
  {
    std::memcpy(dst, pone, pone_size);                          // 复制环形缓冲区的array_one数据到dst中
    std::memcpy(dst + pone_size, ptwo, num_bytes - pone_size);  // 复制环形缓冲区的array_two数据到dst中
  }
}

}
